/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <iostream>
#include <opencv/cv.h>

#include "error/cerror.h"
#include "katanavision/katanafuncs.h"

using namespace std;
using namespace Katana;

//7 : TOWER CONSTRUCTION WITHOUT EYES
int main( int argc, char *argv[] )
{	
	try {
		
		// create katana
		TCdlCOMDesc ccd = {0, 57600, 8, 'N', 1, 300, 0};
		CCdlCOM device(ccd);
		CCplSerialCRC protocol;
		protocol.init(&device);
		Katana::CKatanaSensing katana;
		katana.create ( "katana5M180.cfg", &protocol );
		
		katana.unBlock();
		katana.moveRobotTo(350, 0, 0, 0, 0, 0, true, 5000);
		katana.UpdateSensors();
		katana.movDegrees(M_GRP, 70, true);
		katana.CalculateNoObstacleValues(10);
		katana.movDegrees(M_GRP, 5, true);
		katana.UpdateSensors();
		katana.UpdateSensors();
		
	
		// Start of main algorthm
		cv::Point3f tower(300, 200, - 176); // Location of tower
		SRectF tower_rect(tower.x - 50, tower.y - 50, tower.x + 50, tower.y + 50); // tower rect
		double safeheight = 0; 
		double minz = -176;
		HPlane3f tableplane(0.0f, 0.0f, 1.0f, 186.0f);
		cv::Point2f iobj;
		cv::Point3f obj;
		double object_height;
		double object_sensor_gap = 20;
		
		vector<cv::Point3f> objs;
		objs.push_back(cv::Point3f(400, 0, -186.5));
		objs.push_back(cv::Point3f(300, 0, -186.5));
		objs.push_back(cv::Point3f(400, -100, -186.5));

		for(vector<cv::Point3f>::iterator i = objs.begin() ; i != objs.end(); i++) {
			obj = *i;
			cout << "Try to find object at " << obj << "." << endl;
			obj.z = safeheight;
			do {
				katana.movDegrees(M_GRP, 5, true);
				object_height = FindObjectUnderGripper(katana, obj, safeheight - minz, 10, 5000) - object_sensor_gap;
				if (object_height < 0) {
					cout << "No object at " << obj << "." << endl;
					continue;
				}	
				cout << "Try to grab object at " << obj << ". Its height is " << object_height << "." << endl;
			
				GrabVolObject(katana, object_sensor_gap + 30, 80, 18, 10, 5000);
				//GrabCrash(katana, object_sensor_gap + 30, 80, 5000) ;
				
				if (katana.getMotorEncoders(M_GRP, true) < 2000) {
					cout << "Nothing is grabbed." << endl;
					continue;
				}
				katana.moveRobotTo(obj.x, obj.y, safeheight, 0, 0, 0, true, 5000);
			} while(!(katana.IsObstacle(S_IN_RIGHT_FRONT) && katana.IsObstacle(S_IN_LEFT_FRONT)));
			
			cout << "Try to build at " << tower << "." << endl;
			katana.moveRobotTo(tower.x, tower.y, safeheight, 0, 0, 0, true, 5000);
			tower.z += + object_height;
			katana.moveRobotTo(tower.x, tower.y, tower.z - 25, 0, 0, 0, true, 5000);
			katana.incDegrees(M_GRP, 15, true);
			katana.moveRobotTo(tower.x, tower.y, safeheight, 0, 0, 0, true, 5000);
		}
	}
	catch ( Exception &e ) {
		std::cout << "ERROR: " << e.message() << std::endl;
	}
	catch ( Errors::CError& e ) {
		e.Report(std::cout);
	}
}

